import numpy as np
# pip install matplotlib
import matplotlib.pyplot as plt

Y = np.zeros((5000,), dtype=np.float32)  # 5000个数
R = np.zeros((5000,), dtype=np.float32)  # 5000个数
Δt = 0.001
Kp = 2  # 比例系数
Ti = 0.5  # 积分时间
Td = 1  # 微分时间
rk_1 = 0
rk = 0
y2k_1 = 0

for i in range(0, 5000):
    if (i == 999):
        rk = 1  # 阶跃信号
    y1k = Kp*rk  # 比例环节
    y2k = y2k_1+Δt*rk/Ti  # 积分环节
    y3k = Td*((rk-rk_1)/Δt)  # 微分环节

    Y[i] = y1k+y2k+y3k
    R[i] = rk
    y2k_1 = y2k  # 历史数据
    rk_1 = rk  # 历史数据

t = [i*0.001 for i in range(0, 5000, 1)]  # 5000个数
plt.title("Simulation PID")  # 括号当中输入标题的名称
plt.plot(t, Y, label="Y")
plt.plot(t, R, label="R")
plt.legend(loc='best')  # 图列位置，可选best，center等
plt.show()
